#ifndef _MTSP_HPP
#define _MTSP_HPP

#include<iostream>
#include<algorithm>
#include<unordered_map>
#include<ctime>
using namespace std;

typedef struct FIT{
    int key;
    double val;
} Fit;

bool mycmp(Fit &a, Fit &b){
    return a.val < b.val;
}

int randint(int min_num, int max_num){
    int res = rand() % (max_num - min_num) + min_num;
    return res;
}


class Mtsp
{
private:
    void memwarn(){
        cout<<"Out Of Memory\n";
        exit(-1);
    }
    void dispLog(int, int, double, double);
    int **_cross(const int *, const int *);

private:
    double p_val;
    double best = 1e+9;
public:
    Mtsp(int, int, int, unordered_map<int, int>, double **, int, double);
    ~Mtsp();

public:
    void init_population();
    double _cal_path(const int *);
    double cal_fitness(const int *);

    int **cross(const int *, const int *, int);
    void mutate(int *);

    void _update(int **, int);

    void update(int **, int);

    double step(int);

    double &operator()(int, int);


public:
    // facilities information
    int node_num;
    int car_num;
    int depot_num;
    unordered_map<int,int> car_depot;

    // environment information
    double **graph;

    // Parameters
    int code_length;
    double mr;
    int population_num;

    // variables

    int **individuals;
    Fit *fitness;

};

Mtsp::Mtsp(int node, int car, int depot, unordered_map<int, int> car_depot, double **graph, int num, double mur){
    // init the parameters
    node_num = node;
    car_num = car;
    depot_num = depot;
    car_depot = car_depot;

    this->graph = graph;

    code_length = node + car - 1;

    mr = mur;
    population_num = num;


    p_val = (double)(car - 1) / node_num;
    // init memory

    // individual initialize
    individuals = (int**)malloc(population_num * sizeof(int *));
    if(individuals == nullptr)memwarn();
    int int_unit_size = sizeof(int);

    for(int i = 0; i < population_num; i++){
        int *pointer = (int *)malloc(int_unit_size * code_length);
        if(pointer == nullptr)memwarn();
        individuals[i] = pointer;
    }

    // fitness initialize

    fitness = (Fit *)malloc(population_num * sizeof(Fit));
    if(fitness == nullptr)memwarn();

    cout<<"Parameters Initialized Done!\n";

}


Mtsp::~Mtsp()
{
    if(individuals != nullptr){
        for(int i = 0; i < population_num; i++)free(individuals[i]);
        free(individuals);
    }

    if(fitness != nullptr){
        free(fitness);
    }

    cout<<"Memory Clean Done!\n";
}

double Mtsp::_cal_path(const int *c){
    // 初始化每辆车的行驶路径
    double car_paths[car_num];
    for(int i = 0; i < car_num; i++)car_paths[i] = 0.0;
    // 初始化车辆总路径
    double val = 0.0;

    // calculate
    int car = 0;
    for(int i = 0; i < code_length; i++){
        if(i == 0){
            if(c[i] == -1)car ++;
            else {
                car_paths[car] += graph[car_depot[car]][c[i] + depot_num];
            }
            continue;
        }
        int now = (c[i] == -1)?car_depot[car]:c[i] + depot_num;
        int last = (c[i - 1] == -1)?car_depot[car]:c[i-1]+depot_num;

        if(now != last)car_paths[car] += graph[last][now];

        if(c[i] == -1)car ++;
        
    }
    int last = (c[code_length - 1] == -1) ? car_depot[car_num - 1] : c[code_length - 1] + depot_num;

    if(last >= depot_num)car_paths[car_num - 1] += graph[last][car_depot[car_num - 1]];
    
    for(int i = 0; i < car_num; i++)val += car_paths[i];

    return val;
}

double Mtsp::cal_fitness(const int * c){
    double val = _cal_path(c);

    return val;
}

void Mtsp::_update(int **children, int num){

    for(int i = 0; i < num; i++){
        double val = cal_fitness(children[i]);
        int idx = -1;
        int key = -1;
        for(int j = 0; j < population_num; j++){
            if(val < fitness[j].val){
                key = fitness[population_num - 1].key;
                int *pointer = individuals[key];
                individuals[key] = children[i];
                free(pointer);
                idx  = j;
                break;
            }
        }

        if(idx == -1){
            free(children[i]);
            continue;
        }
        for(int k = population_num - 2; k >= idx; k--){
            fitness[k + 1].key = fitness[k].key;
            fitness[k + 1].val = fitness[k].val;
        }

        fitness[idx].key = key;
        fitness[idx].val = val;
    }

}

void Mtsp::update(int ** children, int num){
    if(children != nullptr)_update(children, num);
    sort(fitness, fitness+population_num, mycmp);
}


void Mtsp::init_population(){
    for(int i = 0; i < population_num; i++){
        int count = 0;
        for(int j = 0; j < node_num; j++){
            individuals[i][j + count] = j;
            double p = (double)rand() / RAND_MAX;

            if(p < p_val && count < car_num - 1){
                count ++;
                individuals[i][j+count] = -1;
            }
        }
        // 补全车辆
        if(count < car_num - 1){
            for(int k = 0; k < car_num - count; k++){
                individuals[i][count + node_num + k] = -1;
            }
        }
        
        int idx1 = randint(0, code_length);
        int idx2 = randint(0, code_length);
        if(idx1 != idx2){
            int t = individuals[i][idx1];
            individuals[i][idx1] = individuals[i][idx2];
            individuals[i][idx2] = t;
        }
    }

    for(int i = 0; i < population_num; i++){
        fitness[i].key = i;
        fitness[i].val = cal_fitness(individuals[i]);
    }

    update(nullptr, 0);
}

int **Mtsp::_cross(const int *p1, const int *p2){

    // apply memory
    int **res = (int **)malloc(2 * sizeof(int *));
    if(res==nullptr)memwarn();
    int int_unit = sizeof(int);
    res[0] = (int *)malloc(code_length * int_unit);
    res[1] = (int *)malloc(code_length * int_unit);
    // copy the parents
    memcpy(res[0], p1, int_unit * code_length);
    memcpy(res[1], p2, int_unit * code_length);

    
    // cross block
    int pos1 = randint(1, code_length);
    int pos2 = randint(1, code_length);

    if(pos1 > pos2){
        pos1 = pos1 + pos2;
        pos2 = pos1 - pos2;
        pos1 = pos1 - pos2;
    }
    else if(pos1 == pos2)return res;

    int length = pos2 - pos1 + 1;
    int block_size = length * sizeof(int);

    int *arr = (int *)malloc(block_size);
    memcpy(arr, p1 + pos1, block_size);
    // calculate the quantity of car
    int num = 0;
    for(int i = 0; i < length; i++){
        if(arr[i] == -1)num ++;
    }
    // core block
    int count = 0;
    int flag = 0;
    for(int i = 0; i < code_length; i++){
        int *p = find(arr, arr + length, p2[i]);
        if(p != arr + length){
            if(p2[i] == -1){
                if(flag < num)flag ++;
                else continue;
            }
            res[0][pos1 + count] = p2[i];
            res[1][i] = arr[count];
            count ++;
        }

        if(count == length)break;

    }

    return res;
}


int ** Mtsp::cross(const int *p1, const int *p2, int num){
    if(num % 2 == 1){
        cout<<"the every scale must be even";
        return nullptr;
    }
    int **res = (int **)malloc(num * sizeof(int *));
    if(res == nullptr)memwarn();

    for(int i = 0; i < num / 2; i++){
        int **every = _cross(p1, p2);
        res[2*i] = every[0];
        res[2*i+1] = every[1];
        free(every);
    }

    return res;

}

void Mtsp::mutate(int *c){
    int pos1 = randint(0, code_length);
    int pos2 = randint(0, code_length);

    if(pos1 != pos2){
        int t = c[pos1];
        c[pos1] = c[pos2];
        c[pos2] = t;
    }
}

double Mtsp::step(int num){
    int idx1 = randint(0, population_num);
    int idx2 = randint(0, population_num);

    // crossAction
    int **children = cross(individuals[idx1],individuals[idx2], num);

    // mutateAction
    for(int i = 0; i < num; i++){
        double p = (double)rand() / RAND_MAX;
        if(p < mr)mutate(children[i]);
    }

    // updateAction
    update(children, num);

    double res = fitness[0].val;

    return res;
}

void Mtsp::dispLog(int step, int epoch, double val, double t){
    if(val < best){
        printf("[%5d/%5d] BestUB = %.2f\t Total Time = %.4f\n", step, epoch, val, t);
        best = val;
    }
}

double & Mtsp::operator()(int epoch, int num){
    cout<<"Iteration Times is "<< epoch<< endl;
    cout<<"Scale size is "<<num<<endl;

    init_population();
    cout<<"Population initializeed Done! Enter the Implemention Phase\n";

    cout<<"Start Timing --- \n";
    clock_t st, et;

    dispLog(0, epoch, fitness[0].val, 0.0);
    st = clock();
    for(int i = 0; i < epoch; i++){
        double val = step(num);
        et = clock();
        dispLog(i+1, epoch, val, (double)(et - st)/CLOCKS_PER_SEC);
    }

    cout<<"the Programming running Done!\n";
    cout<<"the best fitness is "<<fitness[0].val <<endl;
    cout<<"the best individual is :"<<endl;
    int key = fitness[0].key;
    cout<<"[";
    for(int i = 0 ; i < code_length; i++){
        cout<<individuals[key][i]<<" ";
    }
    cout<<"]\n";

    return fitness[0].val;
}

#endif